'''
Created on Mar 29, 2010

@author: jhlee
'''
from Number3D import *

class Quaternion:
    def __init__(self, x=0,y=0,z=0,w=1.0):
        self.x = x
        self.y = y 
        self.z = z
        self.w = w
        
    def copyFrom(self, rhs):
        self.x = rhs.x
        self.y = rhs.y
        self.z = rhs.z
        self.w = rhs.w
        
        
    def toRotationMatrix(self, m):
        m.identity()
        
        Tx = 2.0 * self.x 
        Ty = 2.0 * self.y
        Tz = 2.0 * self.z 
        
        Twx = Tx * self.w 
        Twy = Ty * self.w
        Twz = Tz * self.w
        
        Txx = Tx*self.x 
        Txy = Ty*self.x        
        Txz = Tz*self.x
        
        Tyy = Ty*self.y
        Tyz = Tz*self.y
        Tzz = Tz*self.z
        
        m.n11 = 1.0 - (Tyy+Tzz)
        m.n12 = Txy - Twz 
        m.n13 = Txz + Twy
        
        m.n21 = Txy + Twz
        m.n22 = 1.0 - (Txx+Tzz)
        m.n23 = Tyz-Twx
        
        m.n31 = Txz - Twy
        m.n32 = Tyz + Twx
        m.n33 = 1.0 - (Txx+Tyy)
        
    def rotate(self, v):
        q = Number3D(self.x,self.y,self.z)
        uv = Number3D()
        uuv = Number3D()
        q.cross(v, uv)
        q.cross(uv, uuv)
        uv.scale(2.0*self.w)
        uuv.scale(2.0)
        
        result = Number3D(v.x,v.y,v.z)
        result.add(uv)
        result.add(uuv)
        return result
    
    def multiply(self, rhs):
        w = self.w 
        x = self.x
        y = self.y 
        z = self.z
        
        self.w = w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z
        self.x = w*rhs.x + x*rhs.w + y*rhs.z - z*rhs.y
        self.y = w*rhs.y + y*rhs.w + z*rhs.x - x*rhs.z
        self.z = w*rhs.z + z*rhs.w + x*rhs.y - y*rhs.x
        
        
        
            